#include <dispatch_system/map_pretreatment.h>
#include <schedule_config/config.hpp>

namespace moying
{
namespace navigation
{

int64_t count[10]={0,0,0,0,0,0,0,0,0,0};

using CorrectCells = std::vector<std::vector<CorrectCellInfo>>;

PretreatmentMap::PretreatmentMap(MoyingMap* moying_map,float robot_length,float robot_width,float l_padding,float w_padding){
    moying_map_ptr_ = moying_map;
    robot_length_ = robot_length;
    robot_width_  = robot_width;

    l_padding_ = l_padding;
    w_padding_ = w_padding;
    if(moying_map_ptr_ != nullptr){
        scale_ = moying_map_ptr_->getCostMap()->getResolution() / moying_map_ptr_->getCostMap()->getRawResolution();
        MapDimension d = moying_map->getDimensions();
        raw_coor_in_cell_.resize(d.x * d.y);  
        cell_type_.resize(d.x*d.y); 
    }
}



void PretreatmentMap::setRobotSize(double robot_length,double robot_width,double l_padding,double w_padding){
    robot_length_ = robot_length;
    robot_width_ = robot_width;
    // l_padding_ = l_padding+0.2;
    // w_padding_ = w_padding+0.2;
    l_padding_ = l_padding;
    w_padding_ = w_padding;
    printf("r_w:%.2f,r_l:%.2f,w_pad:%.2f,l_pad:%.2f\n",robot_width_,robot_length_,w_padding_,l_padding_);

}


bool PretreatmentMap::correctMapCellCoordinate(CorrectCells& cci_vec,int x_cell_size,int y_cell_size){

    if(moying_map_ptr_ == nullptr)
        return false;

    CorrectCells& ccis = ccis_;
    int size_x,size_y;
    MapDimension d = moying_map_ptr_->getDimensions();
    size_x = d.x;
    size_y = d.y;

    float max_dis = sqrt(scale_*scale_*2)*4/2;
    printf("==== max_dis : %.4f\n",max_dis);
    auto cost_map_ptr = moying_map_ptr_->getCostMap();
    auto cost_map = cost_map_ptr->get_cost_map();
    float cell_res = moying_map_ptr_->getCostMap()->getResolution();
    float raw_res = moying_map_ptr_->getCostMap()->getRawResolution();
    // auto start = time(0);

    auto construct_cci_vec = [this,d](CorrectCells& cci_vec,const CorrectCells& ccis){

        cci_vec = ccis;

        auto cost_map = moying_map_ptr_->getCostMap();
        int raw_x = cost_map->getRawSizeInCellsX();
        int raw_y = cost_map->getRawSizeInCellsY();

        for(int x=0;x<d.x;x++){
            for(int y=0;y<d.y;y++){
                auto& cci = cci_vec[x][y];
                if(cci.cell_x == -1)
                    continue;
                int theta=0;
               
                for(int th=0;th<4;th++){
                    if(!checkCollision(cci.raw_map_x,cci.raw_map_y,th)){
                        theta |= (1 << th);
                        theta |= ( 1<< (th+4) );
                    }
                }
                if(theta == 0){
                    cci.theta = 0;
                    continue;
                }
                
                cci.theta = theta;

                // cci_vec.emplace_back(cci);
                printf("rl:%.2f rw:%.2f cell:%d,%d raw_map:%d,%d theta:%d cost:%d\n",
                        robot_length(),robot_width(),cci.cell_x,cci.cell_y,cci.raw_map_x,cci.raw_map_y,cci.theta,cci.cost);
            }
        }

        if(cost_calculated())
            return;
        //计算每个cell的cost
        cost_calculated_= true;
        
        // int range = 5;
        // int max_cost = range*2;
        int inflation_num = 1;
        int max_cost = (2*inflation_num+1)*2;
        int min_mx,min_my,max_mx,max_my;
        min_mx=min_my=max_mx,max_my=0;
        for(int rx=0;rx<raw_x;rx++){
            for(int ry=0;ry<raw_y;ry++){
                MAPCOSTS cost = (MAPCOSTS)cost_map->getRawCost(rx,ry);
                if(MAPCOSTS::OBSTACLE != cost)
                    continue;
                int cmx=0;
                int cmy=0;
                if( !cost_map->rawMapToMap(rx,ry,cmx,cmy) )
                    continue;
                // min_mx = cmx-range/2;
                // min_my = cmy-range/2;
                min_mx = cmx-inflation_num;
                min_my = cmy-inflation_num;
                min_mx = min_mx < 0 ? 0 : min_mx;
                min_my = min_my < 0 ? 0 : min_my;
                max_mx = cmx + inflation_num;
                max_my = cmy + inflation_num;
                max_mx = max_mx >= d.x ? d.x-1 : max_mx;
                max_my = max_my >= d.y ? d.y-1 : max_my;
                for(int mx=min_mx;mx<=max_mx;mx++){
                    for(int my=min_my;my<=max_my;my++){
                        auto& cci = cci_vec[mx][my];
                        if(cci.cell_x != -1){
                            int cost = (max_cost - abs(cmx-mx) - abs(cmy-my)) * 2;
                            cost = cost < 0 ? 0 : cost;
                            cci.cost = cci.cost < cost ? cost : cci.cost;
                        }
                    }
                }
                
            }
        }
        
        // for(int x=0;x<d.x;x++){
        //     for(int y=0;y<d.y;y++){
        //         auto& cci = cci_vec[x][y];
        //         if(cci.cell_x == -1)
        //             continue;
        //         // cci_vec.emplace_back(cci);
        //         printf("rl:%.2f rw:%.2f cell:%d,%d raw_map:%d,%d theta:%d cost:%d\n",
        //                 robot_length(),robot_width(),cci.cell_x,cci.cell_y,cci.raw_map_x,cci.raw_map_y,cci.theta,cci.cost);
        //     }
        // }

    };

    if( !ccis.empty() ){
        construct_cci_vec(cci_vec,ccis);
        return true;
    }

    ccis.resize(size_x);
    for(int i=0;i<size_x;i++){
        ccis[i].resize(size_y);
    }

HIGHLIGHT("begin shift cell nav point :"<<time(0)<<" d.x:"<<d.x<<" d.y:"<<d.y);

    int map_offset_x = cost_map_ptr->map_offset_x();
    int map_offset_y = cost_map_ptr->map_offset_y();
    for(int x=0;x<size_x;x++){

        for(int y=0;y<size_y;y++){
            MapCell c;
            c.x=x;
            c.y=y;

            if(getMapCellType(c) != CellType::kInflateCell )
            {
                continue;
            }

            std::vector<MapCellRepel> cell_repels;
            MapCell center_cell;
            center_cell.x = x;
            center_cell.y = y;
            CoordinateInt raw_map_coor;
            if( !cost_map_ptr->mapToRawMap(x,y,raw_map_coor.x,raw_map_coor.y) )
                continue;

            bool ret = getSurroundingMapCells(center_cell,cell_repels);
            float x_offset=0;
            float y_offset=0;
            float x_scale = 0;
            float y_scale = 0;
            char buf[512]={0};
            float max_offset = 0;
            float obs_factor = 0;
            float surround_min_dis= 10000;
            int nearby_cell_num = 0;    //紧临的cell数量
            bool no_obs_surround = true;   //cell周围没有障碍物
            if(ret){
                for(int i=0;i<cell_repels.size();i++){

                    auto cell = cell_repels[i].cell;
                    if( getMapCellType(cell) != kObstacleCell )
                        continue;
                    // if((*cost_map)[cell.x][cell.y] == MAPCOSTS::FREE)
                    //     continue;
                    //检测25宫格如果前面有cell存在障碍物,后面遮挡的cell不处理
                    if(i==0 || i==1 || i==5){
                        auto cell2 = cell_repels[6].cell;
                        if( getMapCellType(cell2) == kObstacleCell )
                            continue;
                    }else if(i==3 || i==4 || i==9){
                        auto cell2 = cell_repels[8].cell;
                        if( getMapCellType(cell2) == kObstacleCell )
                            continue;
                    }else if(i==14 || i==19 || i==20){
                        auto cell2 = cell_repels[15].cell;
                        if( getMapCellType(cell2) == kObstacleCell )
                            continue;
                    }else if(i==18 || i==22 || i==23){
                        auto cell2 = cell_repels[17].cell;
                        if( getMapCellType(cell2) == kObstacleCell )
                            continue;
                    }else if(i==2){
                        auto cell2 = cell_repels[7].cell;
                        if( getMapCellType(cell2) == kObstacleCell )
                            continue;
                    }else if(i==10){
                        auto cell2 = cell_repels[11].cell;
                        if( getMapCellType(cell2) == kObstacleCell )
                            continue;
                    }else if(i==13){
                        auto cell2 = cell_repels[12].cell;
                        if( getMapCellType(cell2) == kObstacleCell )
                            continue;
                    }else if(i==21){
                        auto cell2 = cell_repels[16].cell;
                        if( getMapCellType(cell2) == kObstacleCell )
                            continue;
                    }


                    float min_dis;
                    ret = calculateCellObstacleMinDis(cell,raw_map_coor,min_dis);
                    if(!ret){
                        continue;
                    }
                    
                    // if(x == 19 && y == 32){
                    //     printf("====min dis :%.2f\n",min_dis);
                    // }
                    no_obs_surround = false;

                    surround_min_dis = min_dis < surround_min_dis ? min_dis : surround_min_dis;
                    if(min_dis > max_dis)
                        min_dis = max_dis;
                    float offset = max_dis - min_dis;
                    x_offset += cell_repels[i].x_repel * offset;
                    y_offset += cell_repels[i].y_repel * offset;
                    x_scale += fabs(cell_repels[i].x_repel);
                    y_scale += fabs(cell_repels[i].y_repel);

                    if(offset > max_offset)
                        max_offset = offset;

                    if(i==7 || i==11 || i==12 || i==16)
                        nearby_cell_num++;

                }
            }

            int raw_coor_offset = 0;
            int minx = center_cell.x * scale_-raw_coor_offset+map_offset_x;
            int miny = center_cell.y * scale_-raw_coor_offset+map_offset_y;
            int maxx = (center_cell.x+1) * scale_+raw_coor_offset+map_offset_x;
            int maxy = (center_cell.y+1) * scale_+raw_coor_offset+map_offset_y;

            x_offset = x_scale == 0 ? 0 : x_offset/x_scale;
            y_offset = y_scale == 0 ? 0 : y_offset/y_scale;
            raw_map_coor.x += round(x_offset+0.5);
            raw_map_coor.y += round(y_offset+0.5);


            raw_map_coor.x = raw_map_coor.x >= maxx ? maxx : raw_map_coor.x;
            raw_map_coor.x = raw_map_coor.x < minx ? minx : raw_map_coor.x;
            raw_map_coor.y = raw_map_coor.y >= maxy ? maxy : raw_map_coor.y;
            raw_map_coor.y = raw_map_coor.y < miny ? miny : raw_map_coor.y;

            CorrectCellInfo cci;
            cci.cell_x = center_cell.x;
            cci.cell_y = center_cell.y;
            cci.raw_map_x = raw_map_coor.x;
            cci.raw_map_y = raw_map_coor.y;
            cci.obs_factor  = surround_min_dis == 0 ? 1 : 1/surround_min_dis;

            ccis[x][y] = cci;
        }

    }
HIGHLIGHT("1 end shift cell nav point :"<<time(0));
    for(int x=0;x<x_cell_size;x++){
        for(int y=0;y<y_cell_size;y++){
            if((*cost_map)[x][y] == MAPCOSTS::FREE){
                CorrectCellInfo cci;
                cci.cell_x = x;
                cci.cell_y = y;
                if( !cost_map_ptr->mapToRawMap(x,y,cci.raw_map_x,cci.raw_map_y) )
                    continue;
                cci.obs_factor  = 0;
                cci.cost        = 0;
                // ccis.emplace_back(cci);
                ccis[x][y] = cci;
            }
        }
    }
HIGHLIGHT("2 end shift cell nav point :"<<time(0));



HIGHLIGHT("begin align cell "<<time(0));

    auto cfg = sch::ScheduleConfig::instance()->getConfigInfo();
    if( cfg.traffic_manager_cfg.align_nav_pos )
        alignCellNavPos(ccis,x_cell_size,y_cell_size);

HIGHLIGHT("end align cell "<<time(0));

HIGHLIGHT("begin check collision "<<time(0));

    construct_cci_vec(cci_vec,ccis);

HIGHLIGHT("end check collision "<<time(0));

    return true;
}

void PretreatmentMap::alignCellNavPos(CorrectCells& cci_matrix,int x_cell_size,int y_cell_size){

    int neighbour_len = 6;
    int step = 1;
    int iteration = 4;

    auto alignCell = [=](std::vector<CorrectCellInfo*>cells,bool align_x){
        
        float max_factor = 0;
        float average_x = 0;
        float average_y = 0;
        int num = cells.size();
        CorrectCellInfo* max_weight_cell = nullptr;
        int max_weight_cell_index = 0;

        if(cells.empty() || cells.size() == 1)
            return;

        int i = 0;
        for(auto cell : cells){
            if( cell->obs_factor > max_factor || max_weight_cell == nullptr){
                max_factor = cell->obs_factor;
                max_weight_cell = cell;
                max_weight_cell_index = i;
            }
            if(align_x){
                average_x += cell->raw_map_x;
            }else{
                average_y += cell->raw_map_y;
            }
            i++;
        }
 
        //提高离障碍物近的cell的比重
        if(align_x){
            average_x += max_weight_cell->raw_map_x*num*50;
            average_x /= (num*51);
        }else{
            average_y += max_weight_cell->raw_map_y*num*50;
            average_y /= (num*51);
        }

        // WARNING("max weight cell x:"<<max_weight_cell->cell_x<<" y:"<<max_weight_cell->cell_y);

        int new_x = round(average_x);
        int new_y = round(average_y);

        for(int n=0;n<cells.size();n++){
            auto& cell = cells[n];
            // WARNING("x:"<<cell->raw_map_x<<" y:"<<cell->raw_map_y);
            int factor = abs(n-max_weight_cell_index);
            if(factor <= 2)
                factor = 1;
            else
            {
                factor = factor-1;
            }

            if(average_x > 0){
                int dis = new_x - cell->raw_map_x;
                cell->raw_map_x += dis * 1/factor;
            }else{
                int dis = new_y - cell->raw_map_y;
                cell->raw_map_y += dis * 1/factor;
            }
        }

    };

    while(iteration--){

        for(int x=0;x<x_cell_size;x++){
            for(int y=0;y<y_cell_size;y+=step){
                std::vector<CorrectCellInfo*> cells;
                bool successive = true;
                for(int i=y;i<y+neighbour_len && i<y_cell_size;i++){
                    CorrectCellInfo* p_cell = &cci_matrix[x][i];
                    if(p_cell->cell_x == -1){
                        successive = false;
                        break;
                    }
                    cells.emplace_back(p_cell);
                }
                alignCell(cells,true);
            }
        }

        for(int y=0;y<y_cell_size;y++){
            for(int x=0;x<x_cell_size;x+=step){
                std::vector<CorrectCellInfo*> cells;
                bool successive = true;
                for(int i=x;i<x+neighbour_len && i<x_cell_size;i++){
                    CorrectCellInfo* p_cell = &cci_matrix[i][y];
                    if(p_cell->cell_x == -1){
                        successive = false;
                        break;
                    }
                    cells.emplace_back(p_cell);
                }
                alignCell(cells,false);
            }
        }

    }

}

bool PretreatmentMap::checkCollision(int rx,int ry,int th){

        float width     = (robot_width_ + w_padding_) ;
        float length    = (robot_length_ + l_padding_) ;

        if(th == 1 || th == 3){ //倾斜的矩形检测时宽度加大一点
            // width += 0.2;
        }

        float theta = 0;
        if(th <= 4 && th >= 0 ){
            theta = M_PI_4 * th;
        }else if(th > 4 && th <= 7){
            theta = M_PI_4 * (th-8);
        }
        if(moying_map_ptr_ == nullptr){
            return false;
        }
        double wx,wy;
        moying_map_ptr_->getCostMap()->rawMapToWorld(rx,ry,wx,wy);
        return moying_map_ptr_->checkCollision(length,width,wx,wy,theta);
        
}


bool PretreatmentMap::getSurroundingMapCells(MapCell center_cell,std::vector<MapCellRepel>& surrounding_cells){

    int size_x,size_y;
    MapDimension d = moying_map_ptr_->getDimensions();
    size_x = d.x;
    size_y = d.y;

    if(center_cell.x == 0 || center_cell.x == size_x-1 || center_cell.y == 0 || center_cell.y == size_y-1)
        return false;
    if(getMapCellType(center_cell) == CellType::kObstacleCell)
        return false;

    float r1=1.2;//内圈相邻
    float r2=1.2;//内圈斜向
    float r3=0.5;//外圈相邻
    float r4=0.8;//外圈斜向
    //周围cell从左到右,从上到下排列
    std::pair<float,float> repel[] = {  {r3,-r3},{r3,-r3},{0,-r4},{-r3,-r3},{-r3,-r3},
                                    {r3,-r3},{r1,-r1},{0,-r2},{-r1,-r1},{-r3,-r3},
                                    {r4,0},{r2,0},{-r2,0},{-r4,0},
                                    {r3,r3},{r1,r1},{0,r2},{-r1,r1},{-r3,r3},
                                    {r3,r3},{r3,r3},{0,r4},{-r3,r3},{-r3,r3}
                                };
    int i=0;
    
    for(int y=center_cell.y+2;y>=center_cell.y-2;y--){
        for(int x=center_cell.x-2;x<=center_cell.x+2;x++,i++){
            if(x==center_cell.x && y==center_cell.y){
                i--;
                continue;
            }
            MapCellRepel cell_repel;
            cell_repel.cell.x = x;
            cell_repel.cell.y = y;

            cell_repel.x_repel = repel[i].first;
            cell_repel.y_repel = repel[i].second;
            surrounding_cells.push_back(cell_repel);
        }
    }
    return true;

}

PretreatmentMap::CellType PretreatmentMap::getMapCellType(MapCell cell){

    MapDimension d = moying_map_ptr_->getDimensions();
    int index = cell.x+cell.y*d.x;
    if(index < 0 || index >= cell_type_.size())
        return CellType::kObstacleCell;

    auto& cell_type = cell_type_[index];
    if(cell_type.first)
        return cell_type.second;

    std::vector<CoordinateInt> coors;
// auto start_time = std::chrono::system_clock::now();
    getRawMapCoorInCell(cell,coors);
// count[4] += std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start_time).count();

// start_time = std::chrono::system_clock::now();
    auto raw_cost_map = moying_map_ptr_->getCostMap()->get_raw_cost_map();
// count[5] += std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start_time).count();

    bool inflate = false;
    for(int i=0;i<coors.size();i++){
        auto c = coors[i];
        int x = c.x;
        int y = c.y;
        if((*raw_cost_map)[x][y] == MAPCOSTS::OBSTACLE){
            cell_type.first = true;
            cell_type.second = CellType::kObstacleCell;
            break;
        }else if((*raw_cost_map)[x][y] == MAPCOSTS::INFLATED_OBSTACLE){
            inflate = true;
        }
    }
    
    if(!cell_type.first){
        cell_type.first = true;
        cell_type.second = inflate ? CellType::kInflateCell : CellType::kFreeCell;
    }
    return cell_type.second;

}

bool PretreatmentMap::getRawMapCoorInCell(MapCell cell,std::vector<CoordinateInt>& coors){

    MapDimension d = moying_map_ptr_->getDimensions();
    int index = cell.x+cell.y*d.x;
    if(index < 0 || index >= raw_coor_in_cell_.size()){
        return false;
    }
    auto& coor_vec = raw_coor_in_cell_[index];
    if(coor_vec.size() > 0){
        coors = coor_vec;
        return true;
    }

    auto costmap = moying_map_ptr_->getCostMap();
    int rmx = costmap->getRawSizeInCellsX();
    int rmy = costmap->getRawSizeInCellsY();

    int x_offset = costmap->map_offset_x();
    int y_offset = costmap->map_offset_y();
    int minx = cell.x * scale_+x_offset;
    int miny = cell.y * scale_+y_offset;
    int maxx = (cell.x+1) * scale_+x_offset;
    int maxy = (cell.y+1) * scale_+y_offset;

    minx = minx<0?0:minx;
    miny = miny<0?0:miny;
    maxx = (maxx>=rmx) ? (rmx-1):maxx;
    maxy = (maxy>=rmy) ? (rmy-1):maxy;

    for(int x=minx;x<=maxx;x++){
        for(int y=miny;y<=maxy;y++){
            // printf("coor_vec.size:%d x:%d y:%d\n",coor_vec.size(),x,y);
            auto coor = CoordinateInt(x,y);
            coors.push_back(coor);
            coor_vec.push_back(coor);
        }
    }
    return true;

}

bool PretreatmentMap::calculateCellObstacleMinDis(MapCell cell,CoordinateInt coor,float& min_dis){

    std::vector<CoordinateInt> coors;
    getRawMapCoorInCell(cell,coors);

    auto raw_cost_map = moying_map_ptr_->getCostMap()->get_raw_cost_map();

    min_dis = INT32_MAX;
    bool ret = false;
    for(auto c : coors){
        int x = c.x;
        int y = c.y;
        if((*raw_cost_map)[x][y] != MAPCOSTS::OBSTACLE){
            continue;
        }
        int x_delta = coor.x-x;
        int y_delta = coor.y-y;
        float dis = sqrt(x_delta*x_delta + y_delta*y_delta);
        if(dis < min_dis){
            min_dis = dis;
        }
        ret = true;
    }
    return ret;
}

}

}